#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
 
/**************************************************************************************
 *Function: int pkcs7_padding(char *data, int dataSize, int dataLen)
 *Describe: pkcs7填充
 *Param   : data    : 数据
 *          dataSize: 存储 data的数组容量
 *          dataLen : data 的长度
 *return  ：-1 data数据为空
 *          -2 填充后的数据越界
 *          >0 填充后的数据长度
 ***************************************************************************************/
int pkcs7_padding(char *data, int dataSize, int dataLen)
{
	if(dataLen <= 0)
		return -1;
	
	if((dataLen+16) > dataSize)
		return -2;
	
	uint8_t paddingNum = 16 - (dataLen % 16);
	
	memset(&data[dataLen], paddingNum, paddingNum);
	
	return dataLen+paddingNum;
 
}
 
/**************************************************************************************
 *Function: int pkcs7_cuttingg(char *data, int dataLen)
 *Describe: pkcs7去除填充
 *Param   : data    : 数据
 *          dataLen : data 的长度
 *return  ：-1 data数据为空
 *          -3 填充检查失败，无填充
 *          >0 去除填充后的数据长度
 ***************************************************************************************/
int pkcs7_cuttingg(char *data, int dataLen)
{
	if(dataLen <= 0)
		return -1;
	
	uint8_t paddingNum = data[dataLen-1];
	
	//check
	int i;
	for(i=0;i<paddingNum;i++)
	{
		if(data[dataLen-paddingNum+i] != paddingNum)
			return -3;
	}
	
	memset(&data[dataLen-paddingNum], 0, paddingNum);
	
	return dataLen-paddingNum;
 
}
 
int main(void)
{
    int i;
    char data[128] = {0x01, 0x02, 0x03, 0x04, 0x05};
    int ret = pkcs7_padding(data, sizeof(data), strlen(data));
    printf("pkcs7_padding data(len:%d):\r\n",ret);
    for(i=0;i<ret;i++)
    {
        printf("%02X ", (uint8_t)data[i]);
    }
    printf("\r\n");
 
    int dataLen = ret;
    ret = pkcs7_cuttingg(data, dataLen);
    printf("pkcs7_cuttingg data(len:%d):",ret);
    for(i=0;i<ret;i++)
    {
        printf("%02X ", (uint8_t)data[i]);
    }
    printf("\r\n");
 
    return 0;
}
